// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.org          =
// =               http://www.mahringer.co.at   =
// ==============================================

// Utilities and subroutines

#include "c-ufo.h"
#include "bits.h"

// Math Library
#include "mymath16.h"

void OutSignals(void)
{	
#ifdef ESC_PPM	

	PWM_Val[0] = MVorne;
	PWM_Val[1] = MLinks;
	PWM_Val[2] = MRechts;
	PWM_Val[3] = MHinten;
/*
	// Debug -- Motors off
	PWM_Val[0] = 0;  
	PWM_Val[1] = 0; 	
	PWM_Val[2] = 0;
	PWM_Val[3] = 0;
	PWM_Val[4] = 0;
	PWM_Val[5] = (char) (Vud + _Neutral_Out);
	// PWM_Val[5] = Tl + _Neutral_Out;
	// PWM_Val[4] = MVorne;
*/

	PWM_Val[4] = MCamRoll;
	PWM_Val[5] = MCamPitch;

	
#endif	
}


// convert Roll and Pitch gyro values
// using 10-bit A/D conversion.
// Values are ADDED into RollSamples and PitchSamples
void GetGyroValues(void)
{
#ifdef NO_SENSORS
	return;
#endif

	ADFM = 1;					// select 10 bit mode
	ADCON0 = 0b.10.001.0.0.1;	// select CH1(RA1) Roll

	AcqTime();

	RollSamples = ADRESL;
	RollSamples.high8 = ADRESH;
	
	ADCON0 = 0b.10.010.0.0.1;	// select CH2(RA2) Pitch
	AcqTime();

	PitchSamples = ADRESL;
	PitchSamples.high8 = ADRESH;

	ADCON0 = 0b.10.100.0.0.1;	// select CH4(RA5) Yaw
	AcqTime();
	
	YawSamples = ADRESL;
	YawSamples.high8 = ADRESH;

// pre-flight auto-zero mode	
	if( BiasZeroCount > 0 )
	{
		RollBias += RollSamples;	// adding up 10 bit quantities 
		PitchBias += PitchSamples;
		YawBias += YawSamples;

		// hold all angles at 0 until we have  the bias values collected
		Roll_Angle = 0;
		Pitch_Angle = 0;
		Yaw_Angle = 0;
		LRIntKorr = 0;
		FBIntKorr = 0;

		if( BiasZeroCount == 1 )
		{
			RollBias += (BiasZeroAvgCount/2);
			PitchBias += (BiasZeroAvgCount/2);
			YawBias += (BiasZeroAvgCount/2);
			
			RollBias = RollBias /  BiasZeroAvgCount;	
			PitchBias = PitchBias / BiasZeroAvgCount;
			YawBias =  YawBias /  BiasZeroAvgCount;
		}
		BiasZeroCount--;
	}

}

// Calc the gyro values from added RollSamples 
// and PitchSamples (global variable "nisampcnt")
void CalcGyroValues(void)
{
#ifdef NO_SENSORS
	return;
#endif

	if( BiasZeroCount > 0 )   // No gyro Bias value yet,  Can't fly yet
			return;

	RollSamples -= RollBias;			// Subtract Gyro Bias, makes +/- Delta Theta  in 10 bit resolution 
	PitchSamples -= PitchBias;			// Subtract Gyro Bias, makes +/- Delta Theta  in 10 bit resolution 
	YawSamples -= YawBias;				// same for yaw

// calc Cross flying mode
	if( FlyCrossMode )
	{
// Real Roll = 0.707 * (N + R)
//      Pitch = 0.707 * (N - R)
// the constant factor 0.667 is used instead
		niltemp = RollSamples + PitchSamples;	// 12 valid bits!
		PitchSamples = PitchSamples - RollSamples;	// 12 valid bits!
		RollSamples = niltemp * 2;
		(long)RollSamples /= 3;
		(long)PitchSamples *= 2;
		(long)PitchSamples /= 3;
	}
	
// TODO:   I'm not sure if all this rounding up is necessary -- Need to recompile without and see the diff
	
// Roll
	Roll_Angle += (long) RollSamples;		// Integrate rate of roll into angle
	
	RollSamples += 2;			// round up
	RollSamples >>= 2;			// and use 8 bit for P & D 
	Roll_Rate_8 = (char) RollSamples;			// Roll_Rate_8  has Delta Theta of roll axis in 8 bit representation  
	
	// Erection of angle + compensation for errors from ADC  quantization and BIAS drift
	Roll_Angle += LRIntKorr;
	
// Pitch
	Pitch_Angle += (long) PitchSamples;		// Integrate rate of roll into angle
	
	PitchSamples += 2;	// round up
	PitchSamples >>= 2;				// and use 8 bit for P & D 
	Pitch_Rate_8 = (char) PitchSamples;	

	// Erection of angle + compensation for errors from ADC  quantization and BIAS drift
	Pitch_Angle += FBIntKorr;

// Yaw
	// 10 bit Samples added up + 8 bit IYaw input from stick 
	YawSamples += IYaw;  // Add in Stick input here so that the Yaw Angle can be drivern back to 0 by the yaw control to the motors	
	Yaw_Angle += (long) (YawSamples); // the +- 32K allows for a +- 270 degree variation of the Yaw angle. Since Yaw angle is used to hold heading and the control loop drives it back to 0 this is not a problem 
	YawSamples += 2;			// round up
	YawSamples >>= 2;			// and use 8 bit for P & D 
	Yaw_Rate_8 = (char) YawSamples;	

// Used to be :
//Sum_and_LimitYawAngle();
}



// Mix the Camera tilt channel (Ch6) and the
// ufo air angles (roll and nick) to the 
// camera servos. 
void MixAndLimitCam(void)
{
#ifdef NO_SENSORS
Rp = Np = 0;
#endif

// Calculate Cam Servos use  integral part (direct angle)

	if( BiasZeroCount > 0 ) // while detecting The Zero Bias voltages  do not use the gyros values to correct
	{
//IK7 = IK6 = _Neutral_Out;
		MCamRoll = IK7;
		MCamPitch = IK6;
		return;
	}
	
	// scale and apply the camera compensation factors -- factors used as divisors to avoid overflow problems   
	if (CamRollFactor)
	{
		Rp = Roll_Angle >>1; // Make 9 bits first 
		Rp =  Rp / CamRollFactor;
	}
	else
		Rp=0;

	if (CamPitchFactor)
	{
		Np= Pitch_Angle >>1; // make 9 bit first 
		Np = Np / CamPitchFactor;	
	}
	else
		Np =0;
	
	Rp = Rp + IK7;		// Add in the input channels 
	Np = Np + IK6;		

	// Limit PWM in  Rp because of overflow , Rp is long var
	if( Rp > _Maximum)			// Do this in Rp because of overflow in MCamRoll otherwise
		Rp = _Maximum;
	else if( Rp < _Minimum )
		Rp = _Minimum;
	
	MCamRoll = Rp;			// McamRoll is uint8 
	

	if( Np > _Maximum )
		Np = _Maximum;
	else if( Np < _Minimum )
		Np = _Minimum;
	
	MCamPitch = Np;			//mcamPitch is unit8 
	
}
